#ifndef __sequences_c__ruffi__eurobot08_
#define __sequences_c__ruffi__eurobot08_

#include "motors.c"
#include "time.c"
/*
* otoci robota o 180 stupnu kolem stredu
* asi jete bude potreba doladit konstanty
*/
void turn_180_right( void ) {
	int i;
	for (i = 1; i < 63; i++) {
		right(-i*2);
		left(i*2);
		delay(10);
	}
	for ( i = 63; i > 1; i--) {
		right(-i*2);
		left(i*2);
		delay(10);
	}
	stop();
}

void turn_180_left( void ) {
	int i;
	for (i = 1; i < 62; i++) {
		right(i*2);
		left(-i*2);
		delay(10);
	}
	for ( i = 62; i > 1; i--) {
		right(i*2);
		left(-i*2);
		delay(10);
	}
	stop();
}


void turn_90_right(void) {
	int i;
	for (i = 0; i < 50; i++) { 
		right(-i*2);
		left(i*2);
		delay(10);
	}
	for ( i = 50; i > 0; i--) {
		right(-i*2);
		left(i*2);
		delay(10);
	}
	stop();
}	
		
void turn_90_left(void) {
	int i;
	for (i = 0; i < 49; i++) { 
		right(i*2);
		left(-i*2);
		delay(10);
	}
	for ( i = 49; i > 0; i--) {
		right(i*2);
		left(-i*2);
		delay(10);
	}
	stop();
}	

void turn_45_right(void) {
	int i;
	for (i = 0; i < 43; i++) { 
		right(-i*2);
		left(i*2);
		delay(10);
	}
	for ( i = 42; i > 0; i--) {
		right(-i*2);
		left(i*2);
		delay(10);
	}
	stop();
}	

void turn_45_left(void) {
	int i;
	for (i = 0; i < 43; i++) { 
		right(i*2);
		left(-i*2);
		delay(10);
	}
	for ( i = 42; i > 0; i--) {
		right(i*2);
		left(-i*2);
		delay(10);
	}
	stop();
}

/*******************
**   strategies   **
*******************/
/* stgy_simple() 
*  je nejjednodussi strategie, vyuzivajici predevsim dobreho rozpoznavani micku.
*  Robota je v rohu potreba natocit tak, aby jel k protejsi delsi strane hriste.
*/

/*TODO*/
/*Bez enkoderu zataci doleva, coz je vyhoda pro cerveny tym. Pro modry tym, je jeste treba upravit rychlosti kol, aby zatocil k te stene.*/

/*
* Po narazu do steny zastavi, couvne, otoci se o 180° a couva ke stene,
* dokud se nedotyka obema zadnimi kontakty.
* Pote otevre branu a vysype micky, ktere po ceste nasbiral a vytridil.
* To nacouvavani ke stene by jeste bylo mozne vylepsit tak,
* ze najizdel vicekrat a tou stranou, kterou by narazil by poodjel kousek dal.
* Dale by bylo dobre, kdyby po vysypani micku jel pro dalsi.
*/
void stgy_simple( void ) {
	//jede vpred
	right(100);
	left(100);
	conveyor_up(); //zapne sbirani micku
	//dokud alespon jeden predni dolni spinac nesepne
	while ( (get_front_left()!=0) && (get_front_right()!=0)) { delay(20); }
 	stop();
	conveyor_stop();
	delay(500);
	//couvni
	right(-70);
	left(-70);
	delay(600);
	stop();
	delay(400);
	//otoc se
	turn_180_left();
	stop();
	delay(400);
	//nacouvej k stene
	right(-50);
	left(-50);
	//dokud oba nesepnou
	while ( (get_rear_left()!=0 ) || (get_rear_right()!=0)) {delay(50);}
	stop();
	//vysypat
	gate_up();
	delay(500);
	gate_stop();
	delay(1000);
	balls_inside=0;
	//zavrit
	gate_down();
	delay(500);
	gate_stop();
}
#endif
